#include <PS2X_lib.h>  //for v1.6

/******************************************************************
   set pins connected to PS2 controller:
     - 1e column: original
     - 2e colmun: Stef?
   replace pin numbers by the ones you use
 ******************************************************************/
#define PS2_DAT        12  //14    
#define PS2_CMD        11  //15
#define PS2_SEL        10  //16
#define PS2_CLK        13  //17

/******************************************************************
   select modes of PS2 controller:
     - pressures = analog reading of push-butttons
     - rumble    = motor rumbling
   uncomment 1 of the lines for each mode selection
 ******************************************************************/
//#define pressures   true
#define pressures   false
//#define rumble      true
#define rumble      false

PS2X ps2x; // create PS2 Controller Class

//right now, the library does NOT support hot pluggable controllers, meaning
//you must always either restart your Arduino after you connect the controller,
//or call config_gamepad(pins) again after connecting the controller.

int error = 0;
byte type = 0;
byte vibrate = 0;

// Reset func
void (* resetFunc) (void) = 0;
#include <Servo.h>                       //调用“Servo”库，调制无刷电机PWM输出
Servo ESC;                               //无刷电机命名为“ESC”
String throttle;                         //定义接收字符串为“throttle”
const int buzzer = 2;                    // 定义喇叭引脚为2号数字引脚
const int ledPin = 3;                  // 定义喇叭引脚为3号数字引脚
int mTime;
bool heart = 0;
void setup() {


  ESC.attach(9);                        // 定义无刷电机引脚为9号数字引脚，可以输出PWM
  Serial.begin(9600);                  //设置串口通讯的波特率为9600

  ESC.writeMicroseconds(1000);            //无刷电机初始为低电平
  pinMode(ledPin, OUTPUT);             //前灯继电器输出
  pinMode(buzzer, OUTPUT);             //喇叭输出
  delay(500);  //added delay to give wireless ps2 module some time to startup, before configuring it

  //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************

  //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
  error = ps2x.config_gamepad(PS2_CLK, PS2_CMD, PS2_SEL, PS2_DAT, pressures, rumble);

  if (error == 0) {
    Serial.print("Found Controller, configured successful ");
    Serial.print("pressures = ");
    if (pressures)
      Serial.println("true ");
    else
      Serial.println("false");
    Serial.print("rumble = ");
    if (rumble)
      Serial.println("true)");
    else
      Serial.println("false");
    Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
    Serial.println("holding L1 or R1 will print out the analog stick values.");
    Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
  }
  else if (error == 1)
    Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");

  else if (error == 2)
    Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");

  else if (error == 3)
    Serial.println("Controller refusing to enter Pressures mode, may not support it. ");

  type = ps2x.readType();
  switch (type) {
    case 0:
      Serial.println("Unknown Controller type found ");
      break;
    case 1:
      Serial.println("DualShock Controller found ");
      break;
    case 2:
      Serial.println("GuitarHero Controller found ");
      break;
    case 3:
      Serial.println("Wireless Sony DualShock Controller found ");
      break;
  }
}

void Action(String throttle)//主调函数
{
  if (throttle == "stop")                  //暂停，速度减为零
  {
    Serial.println(" Stop");
    ESC.writeMicroseconds(1000);
    throttle = "0";
  }

  else if (throttle == "one")              //调节滑板速度为1
  {
    int i = ESC.readMicroseconds();
    if (i <= 1350)
      for (i; i <= 1350; i++)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    else
      for (i; i >= 1350; i--)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    Serial.println(" speed 1");
    throttle = "0";
  }

  else if (throttle == "two")              //调节滑板速度为2
  {
    int i = ESC.readMicroseconds();
    if (i <= 1550)
      for (i; i <= 1550; i++)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    else
      for (i; i >= 1550; i--)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    Serial.println(" speed 2");
    throttle = "0";
  }

  else if (throttle == "three")              //调节滑板速度为3
  {
    int i = ESC.readMicroseconds();
    if (i <= 1750)
      for (i; i <= 1750; i++)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    else
      for (i; i >= 1750; i--)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    Serial.println(" speed 3");
    throttle = "0";
  }

  else if (throttle == "four")              //调节滑板速度为4
  {
    int i = ESC.readMicroseconds();
    if (i <= 1950)
      for (i; i <= 1950; i++)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    else
      for (i; i >= 1950; i--)
      {
        ESC.writeMicroseconds(i);
        delay(2);
      }
    Serial.println(" speed 4");
    throttle = "0";
  }

  else if (throttle == "turn on")              //点亮前灯
  {
    digitalWrite(ledPin, HIGH);
    Serial.println(" turn on");
    throttle = "0";
  }

  else if (throttle == "turn off")              //熄灭前灯
  {
    digitalWrite(ledPin, LOW);
    Serial.println(" turn off");
    throttle = "0";
  }

  else if (throttle == "buzzer on")              //铃铛响
  {
    int times = 5;
    Serial.println(" buzzer on");
    for (int i = 0; i <= times; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(300);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

  else if (throttle == "buzzer off")              //铃铛尖悦响
  {
    int times = 2;
    Serial.println(" buzzer off");
    for (int i = 0; i <= times; i++)
    {
      digitalWrite(buzzer, HIGH);
      delay(100);
      digitalWrite(buzzer, LOW);
      delay(200);
    }
    throttle = "0";
  }

}

void loop()
{
  /* You must Read Gamepad to get new values and set vibration values
     ps2x.read_gamepad(small motor on/off, larger motor strenght from 0-255)
     if you don't enable the rumble, use ps2x.read_gamepad(); with no values
     You should call this at least once a second
  */
  if (error == 1) { //skip loop if no controller found
    resetFunc();
  }


  else { //DualShock Controller
    ps2x.read_gamepad(false, vibrate); //read controller and set large motor to spin at 'vibrate' speed

    //    if (ps2x.Button(PSB_START))        //will be TRUE as long as button is pressed
    //    {
    //      Serial.println("Start is being held");
    //      while (1)
    //      {

  }
  ps2x.read_gamepad(false, vibrate);
  if (ps2x.Button(PSB_SELECT))
  {
    Serial.println("Reset");
    digitalWrite(buzzer, LOW);
    digitalWrite(ledPin, LOW);
    ESC.writeMicroseconds(1000);
    //          break;
  }

  if (ps2x.NewButtonState())
  { //will be TRUE if any button changes state (on to off, or off to on)
    if (ps2x.Button(PSB_L3))

    {
      throttle = "turn on";
      Action(throttle);
    }
    if (ps2x.Button(PSB_R3))

    {
      throttle = "turn off";
      Action(throttle);
    }

    if (ps2x.Button(PSB_L2))

    {
      throttle = "buzzer on";
      Action(throttle);
    }

    if (ps2x.Button(PSB_R2))

    {
      throttle = "buzzer off";
      Action(throttle);
    }


  }

  if (ps2x.ButtonPressed(PSB_TRIANGLE))
  {
    throttle = "four";
    Action(throttle);
  }

  if (ps2x.ButtonPressed(PSB_CIRCLE))              //will be TRUE if button was JUST pressed
  {
    throttle = "one";
    Action(throttle);
  }

  if (ps2x.ButtonPressed(PSB_CROSS))              //will be TRUE if button was JUST pressed OR released
  {
    throttle = "two";
    Action(throttle);
  }

  if (ps2x.ButtonPressed(PSB_SQUARE))             //will be TRUE if button was JUST released
  {
    throttle = "three";
    Action(throttle);
  }

  if (ps2x.ButtonPressed(PSB_L1) || ps2x.ButtonPressed(PSB_R1))
  { //print stick values if either is TRUE
    Serial.println(" stop");
    digitalWrite(buzzer, LOW);
    digitalWrite(ledPin, LOW);
    ESC.writeMicroseconds(1000);

  }
  //      }
  delay(50);
}


